function [Y, desc] = generatePoseFeatures( pose, N, opt, headers )
%generatePoseFeatures - Computes a pose feature.

  idx = @(col) getHeaderIdx(col, headers);
  
  KTHseed = 0;
  if (opt == KTHseed)
    % Lhand-to-Lshoulder horizontal displacement
    desc = 'Lhand-Lshoulder-hor-displ';
    Y = pose(1:N,idx('Lhand_x')) - pose(1:N,idx('Lshoulder_x'));
  elseif (opt == KTHseed+1)
    % Rhand-to-Rshoulder horizontal displacement
    desc = 'Rhand-Rshoulder-hor-displ';
    Y = pose(1:N,idx('Rhand_x')) - pose(1:N,idx('Rshoulder_x'));
  elseif (opt == KTHseed+2)
    % Lhand-to-Lshoulder vertical displacement
    desc = 'Lhand-Lshoulder-ver-displ';
    Y = pose(1:N,idx('Lhand_y')) - pose(1:N,idx('Lshoulder_y'));
  elseif (opt == KTHseed+3)
    % Rhand-to-Rshoulder vertical displacement
    desc = 'Rhand-Rshoulder-ver-displ';
    Y = pose(1:N,idx('Rhand_y')) - pose(1:N,idx('Rshoulder_y'));
  elseif (opt == KTHseed+4)
    desc = 'Lelbow-angle';
    Y = getAngle2D(pose, N, 'Lshoulder', 'Lelbow', 'Lhand', idx);
  elseif (opt == KTHseed+5)
    desc = 'Lshoulder-angle';
    Y = getAngle2D(pose, N, 'Neck', 'Lshoulder', 'Lelbow', idx);
  elseif (opt == KTHseed+6)
    desc = 'Relbow-angle';
    Y = getAngle2D(pose, N, 'Rshoulder', 'Relbow', 'Rhand', idx);
  elseif (opt == KTHseed+7)
    desc = 'Rshoulder-angle';
    Y = getAngle2D(pose, N, 'Neck', 'Rshoulder', 'Relbow', idx);
  elseif (opt == KTHseed+8)
    desc = 'Lknee-angle';
    Y = getAngle2D(pose, N, 'Lhip', 'Lknee', 'Lfoot', idx);
  elseif (opt == KTHseed+9)
    desc = 'Rknee-angle';
    Y = getAngle2D(pose, N, 'Rhip', 'Rknee', 'Rfoot', idx);
  elseif (opt == KTHseed+10)
    % Lfoot-to-Lhip horizontal displacement
    desc = 'Lfoot-Lhip-hor-displ';
    Y = pose(1:N,idx('Lfoot_x')) - pose(1:N,idx('Lhip_x'));
  elseif (opt == KTHseed+11)
    % Lfoot-to-Lhip vertical displacement
    desc = 'Lfoot-Lhip-ver-displ';
    Y = pose(1:N,idx('Lfoot_y')) - pose(1:N,idx('Lhip_y'));
  elseif (opt == KTHseed+12)
    % Rfoot-to-Rhip horizontal displacement
    desc = 'Rfoot-Rhip-hor-displ';
    Y = pose(1:N,idx('Rfoot_x')) - pose(1:N,idx('Rhip_x'));
  elseif (opt == KTHseed+13)
    % Rfoot-to-Rhip vertical displacement
    desc = 'Rfoot-Rhip-ver-displ';
    Y = pose(1:N,idx('Rfoot_y')) - pose(1:N,idx('Rhip_y'));
  elseif (opt == KTHseed+14)
    desc = 'Head-torso-hip-angle';
    x1 = pose(1:N,idx('Head_x')) - pose(1:N,idx('Torso_x'));
    y1 = pose(1:N,idx('Head_y')) - pose(1:N,idx('Torso_y'));
    x2 = pose(1:N,idx('Rhip_x')) - pose(1:N,idx('Lhip_x'));
    y2 = pose(1:N,idx('Rhip_y')) - pose(1:N,idx('Lhip_y'));
    Y = angle2D(x1, y1, x2, y2);
  end
  
  % 30-50 = MSRActivity
  MSRseed = 30;
  if (opt == MSRseed)
    desc = 'Left shoulder angle';
    Y = getAngle3D(pose, N, 'Cshoulder', 'Lshoulder', 'Lelbow', idx);
  elseif (opt == MSRseed+1)
    desc = 'Left elbow angle';
    Y = getAngle3D(pose, N, 'Lshoulder', 'Lelbow', 'Lwrist', idx);
  elseif (opt == MSRseed+2)
    desc = 'Left wrist angle';
    Y = getAngle3D(pose, N, 'Lelbow', 'Lwrist', 'Lhand', idx);
  elseif (opt == MSRseed+3)
    desc = 'Left hip angle';
    Y = getAngle3D(pose, N, 'Spine', 'Lhip', 'Lknee', idx);
  elseif (opt == MSRseed+4)
    desc = 'Left knee angle';
    Y = getAngle3D(pose, N, 'Lhip', 'Lknee', 'Lankle', idx);
  elseif (opt == MSRseed+5)
    desc = 'Left ankle angle';
    Y = getAngle3D(pose, N, 'Lknee', 'Lankle', 'Lfoot', idx);
  elseif (opt == MSRseed+6)
    desc = 'Right shoulder angle';
    Y = getAngle3D(pose, N, 'Cshoulder', 'Rshoulder', 'Relbow', idx);
  elseif (opt == MSRseed+7)
    desc = 'Right elbow angle';
    Y = getAngle3D(pose, N, 'Rshoulder', 'Relbow', 'Rwrist', idx);
  elseif (opt == MSRseed+8)
    desc = 'Right wrist angle';
    Y = getAngle3D(pose, N, 'Relbow', 'Rwrist', 'Rhand', idx);
  elseif (opt == MSRseed+9)
    desc = 'Right hip angle';
    Y = getAngle3D(pose, N, 'Spine', 'Rhip', 'Rknee', idx);
  elseif (opt == MSRseed+10)
    desc = 'Right knee angle';
    Y = getAngle3D(pose, N, 'Rhip', 'Rknee', 'Rankle', idx);
  elseif (opt == MSRseed+11)
    desc = 'Right ankle angle';
    Y = getAngle3D(pose, N, 'Rknee', 'Rankle', 'Rfoot', idx);
  elseif (opt == MSRseed+12)
    desc = 'Head-neck-shoulder angle';
    x1 = pose(1:N,idx('Head_x')) - pose(1:N,idx('Cshoulder_x'));
    y1 = pose(1:N,idx('Head_y')) - pose(1:N,idx('Cshoulder_y'));
    z1 = pose(1:N,idx('Head_z')) - pose(1:N,idx('Cshoulder_z'));
    x2 = pose(1:N,idx('Rshoulder_x')) - pose(1:N,idx('Lshoulder_x'));
    y2 = pose(1:N,idx('Rshoulder_y')) - pose(1:N,idx('Lshoulder_y'));
    z2 = pose(1:N,idx('Rshoulder_z')) - pose(1:N,idx('Lshoulder_z'));
    Y = angle3D([x1 y1 z1], [x2 y2 z2]);
  elseif (opt == MSRseed+13)
    desc = 'Head-torso-hip angle';
    x1 = pose(1:N,idx('Head_x')) - pose(1:N,idx('Spine_x'));
    y1 = pose(1:N,idx('Head_y')) - pose(1:N,idx('Spine_y'));
    z1 = pose(1:N,idx('Head_z')) - pose(1:N,idx('Spine_z'));
    x2 = pose(1:N,idx('Rhip_x')) - pose(1:N,idx('Lhip_x'));
    y2 = pose(1:N,idx('Rhip_y')) - pose(1:N,idx('Lhip_y'));
    z2 = pose(1:N,idx('Rhip_z')) - pose(1:N,idx('Lhip_z'));
    Y = angle3D([x1 y1 z1], [x2 y2 z2]);
  elseif (opt == MSRseed+14)
    desc = 'Head-hip-feet angle';
    x1 = pose(1:N,idx('Head_x')) - pose(1:N,idx('Chip_x'));
    y1 = pose(1:N,idx('Head_y')) - pose(1:N,idx('Chip_y'));
    z1 = pose(1:N,idx('Head_z')) - pose(1:N,idx('Chip_z'));
    x2 = (pose(1:N,idx('Lankle_x')) + pose(1:N,idx('Rankle_x'))) ./ 2 - pose(1:N,idx('Chip_x'));
    y2 = (pose(1:N,idx('Lankle_y')) + pose(1:N,idx('Rankle_y'))) ./ 2 - pose(1:N,idx('Chip_y'));
    z2 = (pose(1:N,idx('Lankle_z')) + pose(1:N,idx('Rankle_z'))) ./ 2 - pose(1:N,idx('Chip_z'));
    Y = angle3D([x1 y1 z1], [x2 y2 z2]);
  elseif (opt == MSRseed+15)
    % Torso's vertical compression
    desc = '(Lhip_y + Rhip_y) ./ 2 - (Lshoulder_y + Rshoulder_y) ./ 2';
    Y = (pose(1:N, idx('Lhip_y')) + pose(1:N, idx('Rhip_y'))) ./ 2 - ...
      (pose(1:N, idx('Lshoulder_y')) + pose(1:N, idx('Rshoulder_y'))) ./ 2;
  elseif (opt == MSRseed+16)
    % Legs vertical compression
    desc = '(Lfoot_y + Rfoot_y) ./ 2 - (Lhip_y + Rhip_y) ./ 2';
    Y = (pose(1:N, idx('Lfoot_y')) + pose(1:N, idx('Rfoot_y'))) ./ 2 - ...
      (pose(1:N, idx('Lhip_y')) + pose(1:N, idx('Rhip_y'))) ./ 2;
  elseif (opt == MSRseed+17)
    desc = 'Distance of Lhand to Chip';
    x = pose(1:N, idx('Lhand_x')) - pose(1:N, idx('Chip_x'));
    y = pose(1:N, idx('Lhand_y')) - pose(1:N, idx('Chip_y'));
    z = pose(1:N, idx('Lhand_z')) - pose(1:N, idx('Chip_z'));
    Y = sum(abs( [x y z] ).^2,2).^(1/2);
  elseif (opt == MSRseed+18)
    desc = 'Distance of Rhand to Chip';
    x = pose(1:N, idx('Rhand_x')) - pose(1:N, idx('Chip_x'));
    y = pose(1:N, idx('Rhand_y')) - pose(1:N, idx('Chip_y'));
    z = pose(1:N, idx('Rhand_z')) - pose(1:N, idx('Chip_z'));
    Y = sum(abs( [x y z] ).^2,2).^(1/2);
    
  end
  
end

function i = getHeaderIdx(col, headers)
  i = 1;
  for y=1:length(headers)
    if (strcmp(col, char(headers(y))))
      i = y;
      break;
    end
  end
end

function A = angle3D(a,b)
  A = rad2deg( atan2( sum(abs( cross(a,b) ).^2,2).^(1/2) , dot(a,b,2) ) );
end

function A = angle2D(x1, y1, x2, y2)
  A = rad2deg(atan2(x1.*y2-x2.*y1,x1.*x2+y1.*y2));
end

function A = getAngle3D(data, N, first, mid, last, idx)
  x1 = data(1:N,idx([first '_x'])) - data(1:N,idx([mid '_x']));
  y1 = data(1:N,idx([first '_y'])) - data(1:N,idx([mid '_y']));
  z1 = data(1:N,idx([first '_z'])) - data(1:N,idx([mid '_z']));
  x2 = data(1:N,idx([last '_x'])) - data(1:N,idx([mid '_x']));
  y2 = data(1:N,idx([last '_y'])) - data(1:N,idx([mid '_y']));
  z2 = data(1:N,idx([last '_z'])) - data(1:N,idx([mid '_z']));
  
  A = angle3D([x1 y1 z1], [x2 y2 z2]);
end

function A = getAngle2D(data, N, first, mid, last, idx)
  x1 = data(1:N,idx([first '_x'])) - data(1:N,idx([mid '_x']));
  y1 = data(1:N,idx([first '_y'])) - data(1:N,idx([mid '_y']));
  x2 = data(1:N,idx([last '_x'])) - data(1:N,idx([mid '_x']));
  y2 = data(1:N,idx([last '_y'])) - data(1:N,idx([mid '_y']));
  A = angle2D(x1, y1, x2, y2);
end
